/* 
 * File:   StepDetector.cpp
 * Author: root
 * 
 * Created on October 29, 2015, 8:54 AM
 */

#include "StepDetector.h"

StepDetector* StepDetector::instance = 0;

//StepDetector::StepDetector() {
//    this->a = 0;
//    this->b = 0;
//    this->d = 1.0;
//    this->e = 1.0;
//    
//}

StepDetector::StepDetector() {
    this->a = 0;
    this->b = 0;
    this->d = 1.0;
    this->e = 1.0;
    this->ptrSenA = 0;
    this->ptrSenB = 0;
    this->j = 0;
    this->k = 0;
    this->l = 0;   
}

StepDetector::StepDetector(const StepDetector& orig) {
    this->a = orig.a;
    this->b = orig.b;
    this->d = orig.d;
    this->e = orig.e;
    this->ptrSenA = orig.ptrSenA;
    this->ptrSenB = orig.ptrSenB;
    this->j = orig.j;
    this->k = orig.k;
    this->l = orig.l;
}

StepDetector::~StepDetector() {
}

StepDetector* StepDetector::getInstance()
{
  if ( !StepDetector::instance )
  {
        StepDetector::instance = new StepDetector();
  }
  return StepDetector::instance;
}

StepData* StepDetector::get()
{
    return &this->stepData;
}


void StepDetector::release()
{
    if ( this->getInstance() )
    {
        delete this->getInstance();
    }
}


signed int StepDetector::update(const SensorData *paraSensor)
{
    ThresholdCalculator* pTh;

    int i = 0;
    int result = 0;
    if(this->sensorBuffer.cnt > 0) 
    {
        if(paraSensor->time <= this->sensorBuffer.back()->time 
                || paraSensor->time > this->sensorBuffer.back()->time + 2200)
        {
            this->sensorBuffer.bottom = 0;
            this->sensorBuffer.top = 0;
            this->sensorBuffer.cnt = 0;
        }
    }
    for ( i = 0; ; ++i )
    {
        if (i < this->sensorBuffer.cnt)
        {

            if ( i < 0 ) 
            {
            //print error;
            }
            if (paraSensor->time > (this->sensorBuffer.data[(i+this->sensorBuffer.bottom)%110].time + 2200))
                continue;
        }
        break;
    }
  
  
    if ( i )
    {

        if ( i < this->sensorBuffer.cnt )
        {
            this->sensorBuffer.bottom = (i+this->sensorBuffer.bottom)%110;
            this->sensorBuffer.cnt = this->sensorBuffer.cnt - i;
        }
        else
        {
            this->sensorBuffer.bottom = 0;
            this->sensorBuffer.top = 0;
            this->sensorBuffer.cnt = 0;
        }
    }

    if (this->sensorBuffer.cnt > 109)
    {
        this->sensorBuffer.bottom = (this->sensorBuffer.bottom + 1) % 110;
        this->sensorBuffer.cnt--;
    }

    this->sensorBuffer.data[this->sensorBuffer.top] = *paraSensor;
    this->sensorBuffer.cnt++;
    this->sensorBuffer.top = (this->sensorBuffer.top + 1) % 110;
    
    if(this->ptrSenA && ((paraSensor->time - this->ptrSenA->time) > 2000))
    {
        this->a = 0;
        this->ptrSenA = 0;
        this->l = 0;
    }
    if(this->ptrSenB && ((paraSensor->time - this->ptrSenB->time) > 2000))
    {
        this->a = 0;
        this->ptrSenB = 0;
        this->l = 0;
    }

    if(!this->ptrSenA || (this->ptrSenA->accelerator < paraSensor->accelerator))// TBD maybe wrong here, need to check again
    {
        this->ptrSenA = this->sensorBuffer.back();
        if(this->a != 2)
            this->j = this->sensorBuffer.back();
    }

    if(!this->ptrSenB || (this->ptrSenB->accelerator > paraSensor->accelerator))// TBD maybe wrong here, need to check again
    {
        this->ptrSenB = this->sensorBuffer.back();
        if(this->a != 1)
            this->k = this->sensorBuffer.back();
    }

    if(this->k) 
    {
        if(this->j)
        {
            if (this->l && (this->k->time > this->j->time))
            {
                if((paraSensor->time - this->j->time) > 1200)
                {
                    this->stepData.set(*this->l, *this->j, *this->k);
                    this->a = 0;
                    this->ptrSenA = this->sensorBuffer.back();
                    this->ptrSenB = this->sensorBuffer.back();
                    this->l = 0;
                    return 1;
                }
            }
        }
    }
          
    
    if(this->ptrSenA->time <= this->ptrSenB->time)
    {
        if ((this->ptrSenA->accelerator - this->ptrSenB->accelerator)
              > (this->e * pTh->getInstance()->getThreshold() ))
        {
            this->b = 2;
            if(this->a != 2) 
            {
                this->j = this->ptrSenA;
                this->l = this->k;
                if ((paraSensor->time > (this->stepData.getTime() + 1000)
                        || (this->stepData.getMinLevel() < 80)) 
                        &&((this->j->accelerator - this->k->accelerator) > 130))
                {
                    this->d = 0.5;
                } else {
                    this->d = 1;
                }
                this->e = 0.5;
            }
            this->a = this->b;
            this->ptrSenB = this->sensorBuffer.back();
            this->ptrSenA = this->sensorBuffer.back();
        }
        return 0;
    }

    if ((this->ptrSenA->accelerator - this->ptrSenB->accelerator)
         <= (this->d * pTh->getInstance()->getThreshold()))
    	return result;

    result = 1;
    this->b = 1;
    if((this->a == 1) || (this->k = this->ptrSenB, this->a != 2))
        result = 0;
    else 
        this->stepData.set(*this->l, *this->j, *this->k);
    this->a = this->b;
    this->ptrSenB = this->sensorBuffer.back();
    this->ptrSenA = this->sensorBuffer.back();

    return result;
}




